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CONTROLLING FLEXIBLE ROBOT ARMS USING A HIGH SPEED DYNAMICS PROCESS 

AWARDS ABSTRACT 

A robot manipulator controller for a flexible manipulator 
arm having plural bodies connected at respective movable 
hinges and flexible in plural deformation modes 
corresponding to respective modal spatial influence vectors 
relating deformations of plural spaced nodes of respective 
bodies to the plural deformation modes, operates by 
computing articulated body guantities for each of the 
bodies from respective modal spatial influence vectors, 
obtaining specified body forces for each of the bodies, and 
computing modal deformation accelerations of the nodes and 
hinge accelerations of the hinges from the specified body 
forces, from the articulated body quantities and from the 
modal spatial influence vectors. In one embodiment of the 
invention, the controller further operates by comparing the 
accelerations thus computed to desired manipulator motion 
to determine a motion discrepancy, and correcting the 
specified body forces so as to reduce the motion 
discrepancy. The manipulator bodies and hinges are 

characterized by respective vectors of deformation and 
hinge configuration variables, and computing modal 
deformation accelerations and hinge accelerations is 
carried out for each one of the bodies beginning with the 
outermost body by computing a residual body force from a 
residual body force of a previous body, computing a 
resultant hinge acceleration from the body force, and then, 
for each one of the bodies beginning with the innermost 
body, computing a modal body acceleration from a modal body 
acceleration of a previous body, computing a modal 
deformation acceleration and hinge acceleration from the 
resulting hinge acceleration and 
acceleration . 
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CONTROLLING FLEXIBLE ROBOT ARMS USING A HIGH SPEED 

DYNAMICS PROCESS 


BACKGROUND OF THE INVENTION 


Origin of the Invention: 

10 

The invention described herein was made in the performance of work under a 
NASA contract, and is subject to the provisions of Public Law 96-517 (35 USC 202) in 
which the contractor has elected not to retain title. 

Technical Field: 

15 

The invention relates to robot manipulators and more particularly to a method 
and apparatus for controlling robot arms having flexible links using a high speed recursive 
dynamics algorithm to solve for the accelerations of link deformation and hinge rotations 
from specified body forces applied to the links. 

Background Art: 

Controlling robot manipulator arms is a well-known problem and has been de- 
scribed in a number of publications. The invention herein will be described with reference 
to the following publications by referring to each publication by number, such as Ref.[l], 
25 Ref. [2], or simply [1] or [2], for example. 
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The invention uses spatial operators to develop new spatially recursive dynamics 
algorithms for flexible multibody systems. The operator description of the dynamics is 
identical to that for rigid multibody systems. Assumed-mode models are used for the 
deformation of each individual body. The algorithms are based on two spatial opera- 
tor factorizations of the system mass matrix. The first (Newton-Euler) factorization 
of the mass matrix leads to recursive algorithms for the inverse dynamics, mass matrix 
evaluation, and composite-body forward dynamics for the system. The second (Inno- 
vations) factorization of the mass matrix, leads to tin operator expression for the mass 
matrix inverse and to a recursive articulated-body forward dynamics algorithm. The 
primary focus is on serial chains, but extensions to general topologies are also described. 

A comparison of computational costs shows that the articulated-body forward dynamics 
algorithm is much more efficient than the composite-body algorithm for most flexible 
multibody systems. 
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1. Nomenclature 


We use coordinate-free spatial notation ( [1, 2]) in this specification. A spatial velocity 
of a frame is a 6-dimensional quantity whose upper 3 elements are the angular velocity 
5 and whose lower 3 elements are the linear velocity. A spatial force is a 6-dimensional 

quantity whose upper 3 elements are a moment vector and whose lower 3 elements are a 
force vector. 


10 


15 


A variety of indices are used to identify different spatial quantities. Some examples 
are: V s (jk) is the spatial velocity of the j th node on the k th body; V s (k) — coljv^j*)} is 
the composite vector of spatial velocities of all the nodes on the k th body; V s = col j 

is the vector of spatial velocities of all the nodes for all the bodies in the serial chain. The 
index k will be used to refer to both the k th body as well as the k tfl body reference frame 
Tki with the usage being apparent from the context. Some key quantities are defined 
below in accordance with Figures la and lb. 


General Quantities: 


20 


25 


30 


x = [x] x € 3? 3x3 - the skew-symmetric cross-product matrix associated with 
the 3-dimensional vector x 
. da: 

x = — — the time derivative of x with respect to an inertial frame 
dt 


diagjx(fc) j 
col|x(/:)| 

<t>( x ,y) 


Individual 


- the time derivative of x with respect to the body-fixed (rotating) frame 
- a block diagonal matrix whose k th diagonal element is x(A;) 

- a column vector whose k th element is x(k) 

G Sf 3 - the vector from point /frame x to point /frame y 

^ € 3? 6x6 - the spatial transformation operator which trans- 
forms spatial velocities and forces between points/frames x and y 

Body Nodal Data: 


/ J K x , y) 
{* 1 


n a (k) - number of nodes on the k th body 

Tk - body reference frame with respect to which the deformation field for the 
k th body is measured. The motion of this frame characterizes the motion of 
the k th body as a rigid body. 
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jk - j th node on the k th body 
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l o(kJ k ) € S 3 - vector from Tk to the location (before deformation) of the j th node 
reference frame on the k th body 

f>i(jk) G 9R 3 - translational deformation of the j ih node on the k th body 

Kk,jk) = lo(k,jk) + S,(Jk) G 9R 3 - vector from Tk to the location (after deformation) 
of the j th node reference frame on the k th body 

$u(jk) G Sf 3 - deformation angular velocity of the j th node on the k th body with 
respect to the body frame Tk 

*"Uk) € 3R 3 - deformation linear velocity of the j th node on the k th body with 

respect to the body frame Tk 

u {jk) G - the spatial displacement of node jk- The translational component of 
u (jk) is h(jk), while its time derivative with respect to the body frame Tk is 

s*Uk) \ 


Hjk) = 


6v(jk) 


^J{jk) G 3? 3x3 - inertia tensor about the nodal reference frame for the j th node on 
the k th body 

p(jk) G ft 3 - vector from the nodal reference frame to the node center of mass for 
the j th node on the k th body 

m(jk) - mass of the j th node on the k th body 

Jin) 




mi 


tik)p(jk) \ ^ . . . 

v -m(j k )p(j k ) m(j k )I ) e ^ ' Spatial ,nertia about the nodaI 
reference frame for the j th node on the k ih body 

M a {k) diag{A/,(>*)} € »«».(fc)x6n.(*) . structural mass matrix for the k th body 

K,(k) G 3 J 6n »(*)x 6 n.(fc) structural stiffness matrix for the k th body 

Individual Body Modal Data: 

n m (k) - number of assumed modes for the k tk body 

Af(/:) = n m (k) + 6 - number of deformation plus rigid-body degrees of freedom for 

the k th body 

p(k) e - vector of modal deformation variables for the k th body 
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€ 9ft 6 - modal spatial displacement vector for the r th mode at the jfj 1 nodal 
reference frame 

n j (fc) = [rij (Ar) , • • • , G ?R 6xnm ( k ) - modal spatial influence vector for the 

jk node. The spatial deformation of node jk is given by u(jk) = U 3 (k)T](k). 

I1(A:) = col{n j (fc) j G 38 6n «(*) xn «( fc ) - the modal matrix for the k th body. The r th 

column of II(fc) is denoted II r (fc) € S 6 "*^ and is the mode shape function 
for the r th assumed mode for the k th body. The deformation field for the 
k th body is given by u(k) = II(fc)?7(A:), while u(k ) = Il(k)r)(k). 

M m {k) € . modal mass matrix for the k th body. 

Km{k) £ . modal stiffness matrix for the k th body. 

Multibody Data: 


15 


20 
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30 


N - number of bodies in the serial flexible multibody system 

^ = ■ overall degrees of freedom in the serial chain obtained by 

disregarding the hinge constraints 

n r (k) - number of degrees of freedom for the k th hinge 

Af(k) TL m (k'j + n r (k) - number of deformation plus hinge degrees of freedom for 
the k th body 

^ ~ ‘ overall deformation plus hinge degrees of freedom for the 

serial chain 

dk - node on the k th body to which the k th hinge is attached 

tk - node on the k th body to which the ( k — l) 1/l hinge is attached 

Ok - reference frame for the k th hinge on the k th body. This frame is fixed to 
node dk. 

°t - reference frame for the k th hinge on the (k + l) tA body. This frame is fixed 
to node t k+ j. 

0(k) G 8 ! ? nr( ^ - vector of configuration variables for the k th hinge 

P(k) G - vector of generalized velocities for the k th hinge 
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Ay(fc) = ( J G 9R 6 - relative spatial velocity for the k th hinge defined as the 

V Av ( k > ) 


H*(k) 


spatial velocity of frame Ok with respect to frame 0£ 

G §ff 6xnr ^l - joint map matrix for the k th hinge, whose columns comprise the 
unit vectors of the hinge. We have that A v{k) = H*(k)fl(k). 


G sfcVW . vector of (deformation plus hinge) generalized config- 


uration variables for the k ih body 

( *?(£) \ 

I G ^Sir^ - vector of (deformation plus hinge) generalized veloc- 

0 ( k ) ) 

ities for the k th body 


V(k ) = V (T k ) = I G S? 6 - spatial velocity of the k th body reference frame 

\ v ( k ) / 

T\ t, with u>(k) and v(k) denoting the angular and linear velocities respectively 
of frame Tk 

V(Ok) G - spatial velocity of frame Ok 

V (0 £ ) G 9R 6 - spatial velocity of frame 0% 

V>{jk) G 3ff 6 - spatial velocity of the j ih node on the k th body. 

Q a{jk) € J? 6 - spatial acceleration of the j th node on the k th body. 


Vm(k) = 


G - modal spatial velocity of the k th body 


Q m(^) = V m (k) G - modal spatial acceleration of the k th body 

a m (k) G - modal Coriolis and centrifugal accelerations for the k th body 

b m (k) G - modal gyroscopic forces for the k th body 

fm(k) G - modal spatial force of interaction between the k th and 

(k + l) t/l bodies 


fs{jk) € 9? 6 - spatial force at node jk 
f(k ) G 9£ 6 - effective spatial force at frame Tk 
T(k ) G - generalized force for the k th body 
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Hjr(k) = H(k)<f>(O k ,k) £ 9? nr( * )x6 - joint map matrix referred to frame T k for the 
k th hinge 

H{k) = [ ] £ 3^( fc )x^( fc ) - (deformation plus hinge) modal joint 

\ 0 Hjr{k) ) 


map matrix for the k th body 


A(k) = 


[n*(fc)]- 


4>{k,t k ) 

t k and frame T k 


£ 9^ ( * )x6 - relates spatial forces and velocities between node 


#(fc + l,fc) = [0, <^(4+i5 £)] €E 3? 6x ^^ - relates spatial forces and velocities between 

node t k+ 1 and frame T k 

^(A: + 1,1b) =A{k+l)B(k + l,k)= I ° [ n '(^ + l)W*+i,A:) \ 6 ^(fc+Dx^fc) . the 

y 0 <f>(k + 1, k) J 

interbody transformation operator which relates modal spatial forces and 
velocities between the k th and ( k + l) lfc bodies 

t 0 \ 


C(k, k — l) = 


4>{t k ,k- 1) 




V 0 / 

B(k) = [<f>{k, 1 *), <f>(k, 2 k ), • • • , <£(&,«,(&))] £ 0j6x6n,(fc) . relates the spatial velocity 
of frame T k to the spatial velocities of all the nodes on the k th body when 
the body is regarded as being rigid 

-M £ 3?^ xAr - the multibody system mass matrix 

C £ 3^ - the vector of Coriolis, centrifugal and elastic forces for the multibody 
system 


2. Introduction 


The invention uses spatial operators ( [1, 2]) to formulate the dynamics and develop 
efficient recursive algorithms for flexible multibody systems. Flexible spacecraft, limber 
space manipulators, and vehicles are important examples of flexible multibody systems. 
Key features of these systems are the large number of degrees of freedom and the com- 
plexity of their dynamics models. 
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Some of the goals of the invention are: (1) providing a high- level architectural 
understanding of the structure of the mass matrix and its inverse; (2) showing that the 
high-level expressions can be easily implemented within the very well understood Kalman 
filtering and smoothing architecture; (3) developing very efficient inverse and forward 
dynamics recursive algorithms; and (4) analyzing the computational cost of the new 
algorithms. Accomplishing these goals adds to the rapidly developing body of research 
in the recursive dynamics of flexible multibody systems (see [3, 4, 5]). 

It is assumed that the bodies undergo small deformations so that a linear model 
for elasticity can be used. However, large articulation at the hinges is allowed. No special 
assumptions are made regarding the geometry of the component bodies. To maximize 
applicability, the algorithms developed here use finite-element and/or assumed-mode 
models for body flexibility. For notational simplicity, and without any loss in generality, 
the main focus of this specification is on flexible multibody serial chains. Extensions to 
tree and closed-chain topologies are discussed. 

In Section 3 we derive the equations of motion and recursive relationships for 
the modal velocities, modal accelerations, and modal forces. This section also contains 
a derivation of the Newton-Euler Operator Factorization of the system mass matrix. A 
recursive Newton-Euler inverse dynamics algorithm to compute the vector of generalized 

forces corresponding to a given state and vector of generalized accelerations is described 
in Section 4. 

In Section 5, the Newton-Euler factorization of the mass matrix is used to develop 
a partly recursive composite-body forward dynamics algorithm for computing the gener- 
alized accelerations of the system. The recursive part is for computing the multibody 
system mass matrix. This forward dynamics algorithm is in the vein of well-established 
approaches ( [6, 7]) which require the explicit computation and inversion of the system 
mass matrix. However, the new algorithm is more efficient because the mass matrix is 
computed recursively and because the detailed recursive computations follow the high- 
level architecture (i.e. roadmap) provided by the Newton-Euler factorization. 

In Section 6 we derive new operator factorization and inversion results for the 
mass matrix that lead to the recursive articulated-body forward dynamics algorithm. A 
new mass matrix operator factorization, referred to as the Innovations factorization, 
is developed. The individual factors in the innovations factorization are square and 




invertible operators. This is in contrast to the Newton-Euler factorization in which the 
factors are not square and therefore not invertible. The Innovations factorization leads 
to an operator expression for the inverse of the mass matrix. Based on this expression, 
in Section 7 we develop the recursive articulated body forward dynamics algorithm for 
the multibody system. This algorithm is an alternative to the composite-body forward 
dynamics algorithm and requires neither the explicit formation of the system mass matrix 
nor its inversion. The structure of this recursive algorithm closely resembles those found 
in the domain of Kalman filtering and smoothing ( [8]). 

In Section 8 we compare the computational costs for the two forward dynamics 
algorithms. It is shown that the articulated body forward dynamics algorithm is much 
more efficient than the composite body forward dynamics algorithm for typical flexible 
multibody systems. In Section 9 we discuss the extensions of the formulation and 
algorithms in this specification to tree and closed-chain topology multibody systems. 

SUMMARY OF THE INVENTION 

A robot manipulator controller for a flexible manipulator arm having plural bodies 
connected at respective movable hinges and flexible in plural deformation modes corre- 
sponding to respective modal spatial influence vectors relating deformations of plural 
spaced nodes of respective bodies to the plural deformation modes, operates by com- 
puting articulated body quantities for each of the bodies from respective modal spatial 
influence vectors, obtaining specified body forces for each of the bodies, and computing 
modal deformation accelerations of the nodes and hinge accelerations of the hinges from 
the specified body forces, from the articulated body quantities and from the modal spatial 
influence vectors. In one embodiment of the invention, the controller further operates by 
comparing the accelerations thus computed to desired manipulator motion to determine 
a motion discrepancy, and correcting the specified body forces so as to reduce the motion 
discrepancy. 

Computing the articulated body quantities is carried out for each body beginning 
at the outermost body by computing a modal mass matrix, computing an articulated 
body inertia from the articulated body inertia of a previous body and from the modal 
mass matrix, computing an articulated hinge inertia from the articulated body inertia, 
computing an articulated body to hinge force operator from the articulated hinge inertia, 
computing a null force operator from the articulated body to hinge force operator. This 
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is followed by revising the articulated body inertia by transforming it by the null force 
operator. 

The manipulator bodies and hinges are characterized by respective vectors of 
deformation and hinge configuration variables, and computing modal deformation accel- 
erations and hinge accelerations is carried out for each one of the bodies beginning with 
the outermost body by computing a residual body force from a residual body force of 
a previous body and from the vector of deformation and hinge configuration variables, 
computing a resultant hinge acceleration from the body force, the residual body force 
and the articulated hinge inertia, and then, for each one of the bodies beginning with 
the innermost body, by computing a modal body acceleration from a modal body ac- 
celeration of a previous body, computing a modal deformation acceleration and hinge 
acceleration from the resulting hinge acceleration and from the modal body acceleration 
transformed by the body to hinge force operator. 

Computing a resultant hinge force is followed by revising the residual body force 
by the resultant hinge force transformed by the body to hinge force operator, and com- 
puting a modal deformation acceleration and hinge acceleration is followed by revising 
the modal body accleration based upon the modal deformation and hinge acceleration. 
The computing is performed cyclically in a succession of time steps, and the vector of 
deformation and hinge configuration variables is computed from the modal deformations 
and hinge accelerations of a previous time step, or is derived by reading robot joint 
sensors in real time. 

In a preferred embodiment, the articulated body inertia, the articulated hinge 
inertia, the body to hinge force operator, the null force operator, the body force, the 
residual body force, the resultant hinge acceleration and the resultant hinge force are 
each partitioned into free and rigid versions. This embodiment operates by computing the 
flexible version of the resultant hinge force from the applied body force, and computing 
the flexible version of the residua] body force and from the rigid version of the residual 
body force transformed by the modal spatial influence vector. The articulated body 
inertia is decomposed into rigid-free and rigid-rigid coupling components, and the rigid 
version of the residual body force is revised based upon a function of the rigid-rigid and 
rigid-free coupling components of the articulated body inertia and a flexible version of the 
articulated body inertia. This embodiment decomposes the manipulator’s modal mass 
matrix into rigid-free and rigid-rigid coupling components and computes the rigid-rigid 
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and rigid-free coupling components of the articulated body inertia from respective ones 
of the rigid-rigid and rigid- free coupling components of the modal mass matrix. 

In this embodiment, free and rigid versions of a deformation and hinge modal joint 
map matrix are computed for each body so that the flexible version of the articulated 
hinge inertia is computed from the articulated body inertia transformed by the flexible 
version of the corresponding deformation and hinge modal joint map matrix, the rigid 
version of the articulated body inertia is computed from a function of the rigid-rigid 
and rigid-free coupling components of the articulated body inertia transformed by the 
flexible version of the corresponding deformation and hinge modal joint map matrix, 
the rigid version of the articulated hinge inertia is computed from the rigid version of 
the articulated body inertia, and the rigid version of the body to hinge force operator 
is computed from the rigid versions of the articulated body inertia and the articulated 
hinge inertia. The free and rigid versions of the deformation and hinge modal joint map 
matrix are formed by computing a joint map matrix corresponding to unit vectors of the 
hinges and computing the deformation and hinge modal joint map matrix from the joint 
map matrix and from the modal spatial influence vector. 

In this embodiment, the flexible version of the resulting hinge acceleration is 
computed from the flexible versions of the articulated hinge inertia and resulting hinge 
force, and the rigid version of the resulting hinge acceleration is computed from the rigid 
versions of the articulated hinge inertia and resulting hinge force. The residual body 
force is revised in this embodiment by adding to the residual body force a product of the 
rigid versions of the resultant hinge force and the body to hinge force operator. 
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BRIEF DESCRIPTION OF THE DRAWINGS 


30 


Figure la is a simplified diagram of a portion of a robot manipulator having 
flexible links, and illustrating the coordinate system employed in one embodiment of the 
invention. 

Figure lb is a simplified diagram illustrating the finite element analysis employed 
in the invention, in which the displacement of plural spaced nodes along the length of a 
flexible link follows a well- recognized pattern for each mode of flexibility. 

Figure 2 is a block diagram illustrating how the articulated body quantities are 
produced in one embodiment of the invention. 



Figure 3 is a block diagram illustrating an articulated body forward dynamics 
algorithm for flexible link manipulators in accordance with the present invention. 

Figure 4 is a block diagram illustrating the process of the invention for controlling 
a robot manipulator having flexible links. 

Figures 5a and 5b constitute a block diagram illustrating a preferred embodiment 
of the articulated body forward dynamics algorithm employed in the process of Figure 4. 

Figure 6 is a simplified schematic block diagram of apparatus embodying the 
present invention. 

DETAILED DESCRIPTION OF THE INVENTION 

3. Equations of Motion for Flexible Serial Chains 

In this section, we develop the equations of motion for a serial flexible multibody system 
with N flexible bodies. Each flexible body is assumed to have a lumped mass model 
consisting of a collection of nodal rigid bodies. Such models are typically developed 
using standard finite element structural analysis software. The number of nodes on the 
k th body is denoted n s (k). The j th node on the k th body is referred to as the jj/ 1 node. 
Each body has associated with it a body reference frame, denoted Tk for the k th body. The 
deformations of the nodes on the body are described with respect to this body reference 
frame, while the rigid body motion of the k th body is characterized by the motion of 
frame Tk- 

The 6-dimensional spatial deformation (slope plus translational) of node jk (with 
respect to frame J-k) is denoted u(jk) £ 5? 6 . The overall deformation field for the k th body 
is defined as the vector u(k) = col|u(jfc)| £ The vector from frame Tk to the 

reference frame on node jk is denoted l(k,jk) € 9R 3 . 

With M„(jk) G ft 6 * 6 denoting the spatial inertia of the j th node, the struc- 
tural mass matrix for the k th body M t (k) is the block diagonal matrix diag{M,(; fc )} € 
jj6n,(fc)x6n < (fc) The ^rucfura/ stiffness matrix is denoted K s (k) 6 9t en *(*)x6M*) > Both 
M 3 (k) and I< s (k) are typically generated using finite element analysis. 

As shown in Figure la, the bodies in the serial chain are numbered in increasing 
order from tip to base. We use the terminology inboard (outboard) to denote the direction 



along the serial chain towards (away from) the base body. The k tfl body is attached on 
the inboard side to the ( k -+■ l) </l body via the k th hinge, and on the outboard side to 
the ( k — l) th body via the (k — l) tfc hinge. On the k th body, the node to which the 
outboard hinge (the ( k — l) i/l hinge) is attached is referred to as node t k , while the node 
to which the inboard hinge (the k th hinge) is attached is denoted node d k . Thus the 
k th hinge couples together nodes d k and f^+i* Attached to each of these pair of adjoining 
nodes are the k ^ hinge reference frames denoted (D k and C) k , respectively. The number 
of degrees of freedom for the k th hinge is denoted n r (k). The vector of configuration 
variables for the k 1 ^ hinge is denoted 0(k) 6 while its vector of generalized speeds 

is denoted 0{k) € 9£" r ^. In general, when there are nonholonomic hinge constraints, 
the dimensionality of f3(k) may be less than that of 0(k). For notational convenience, 
and without any loss in generality, it is assumed here that the dimensions of the vectors 
0(k) and 0(k) are equal. In most situations, fl(k) is simply 0. However there are many 
cases where the use of quasi-coordinates simplifies the dynamical equations of motion 
and an alternative choice for /?(&) may be preferable. The relative spatial velocity A v(k) 
across the hinge is given by H m (k)0(k), where H*(k) denotes the joint map matrix for 
the k ih hinge. 

Assumed modes are typically used to represent the deformation of flexible bodies, 
and there is a large body of literature dealing with their proper selection. There is however 
a close relationship between the choice of a body reference frame and the type of assumed 
modes. The complete motion of the flexible body is contained in the knowledge of the 
motion of the body reference frame and the deformation of the body as seen from this 
body frame. In the multibody context, it is often convenient to choose the location of 
the k th body reference frame T k as a material point on the body and fixed to node d k 
at the inboard hinge. For this choice, the assumed modes are cantilever modes and node 
d k exhibits zero deformation ( u(d k ) = 0). Free-free modes are also used for representing 
body deformation and are often preferred for control analysis and design. For these 
modes, the reference frame !F k is not fixed to any node, but is rather assumed to be 
fixed to the undeformed body, and as a result all nodes exhibit nonzero deformation. 
The dynamics modeling and algorithms developed here handle both types of modes, 
with some additional computational simplifications arising from Eq. (1) when cantilever 
modes are used. For a related discussion regarding the choice of reference frame and 
modal representations for a flexible body see [9], 

We assume here that a set of rc m (fc) assumed modes has been chosen for the 



k tfl body. Let € 3J 6 denote the modal spatial displacement vector at the jfj 1 node 

for the r th mode. The modal spatial displacement influence vector IP (A) G 3? 6x ”"*(*) for 
the jl k node and the modal matrix II(fc) G §fc 6n *(*)* w m(*) f or t |j e j .th b 0( jy are defined as 
follows: 

W(k)= [ni(*) r .-X m( *)W] and U(k) = co\{w(k)} 

The r th column of IT(&) is denoted IT r ( Ar) and defines the mode shape for the r th assumed 
mode for the k th body. Note that for cantilever modes we have 

U d T (k) = 0 for r — 1 • • • n m (k) (1) 

With r](k) G denoting the vector of modal deformation variables for the k th body, 

the spatial deformation of node j k and the spatial deformation field it(k) for the k th body 
are given by 

u (jk) = U j (k)r)(k) and u(k) = II(A:)7/(A) (2) 

The vector of generalized configuration variables 1 9(k) and generalized speeds x(k) 
for the k th body are defined as 

m = ( i\k ) ) 6 *<*> - ( i\ k k) ) ^ ^ 

where Af(k) — n m (k ) + n r (k). The overall vectors of generalized configuration variables 
$ and generalized speeds \ for the serial multibody system are given by 

d = col jt9(&) j G ^ and \ = col{x(£)j 6 ^ (4) 

where Af = 22*= i V(£) denotes the overall number of degrees of freedom for the multi- 
body system. The state of the multibody system is defined by the pair of vectors 
For a given system state the equations of motion define the relationship between 

the vector of generalized accelerations \ and the vector of generalized forces T G 9^ for 
the system. The inverse dynamics problem consists of computing the vector of general- 
ized forces T for a prescribed set of generalized accelerations x ■ The forward dynamics 
problem is the converse one and consists of computing the set of generalized accelerations 
X resulting from a set of generalized forces T. The equations of motion for the system 
are developed in the remainder of this section. 

3.1 Recursive Propagation of Velocities 

Let V(k) G 3f 6 denote the spatial velocity of the k th body reference frame T k . The 
spatial velocity V s (t k+ \) G ft 6 of node t k+i (on the inboard of the k ih hinge) is related to 
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the spatial velocity V(k + 1) of the (k + \) th body reference frame J- k +i, and the modal 
deformation variable rates ri(k + 1) as follows: 


5 


10 


v,(t k+l ) = <f> m (k + l,t k+1 )V(k+\) + u(t k+ i) 

= 4>'(k 4- l,t k+ i)V(k + 1) + n \k + l)r,(k + 1) (5) 


The spatial transformation operator <f>(x,y) € 3?® x6 above is defined to be 


»-'■(: "r’) 


( 6 ) 


where /(x, y) G J? 3 denotes the vector between the points x and y. Note that the following 
important (group) property holds: 


<j)(x,y)<t>(y,z) = <f>(x,z) 

for arbitrary points x, y and z. As in Eq. (5), and throughout this specification, the 
15 index k will be used to refer to both the k tk body as well as to the k ih body reference 

frame T k with the specific usage being evident from the context. Thus for instance, V(k) 
and <f>(k,t t) are the same as and 4>{!Fkit k ) respectively. 

The spatial velocity V(0£) of frame O % (on the inboard side of the k ih hinge) is 
related to V a (t k+ 1 ) via 

V(Ot) = f(t M ,O k )V,(t M ) (7) 

Since the relative spatial velocity A v(k) across the k th hinge is given by H*(k)fl(k), the 
spatial velocity V(O k ) of frame O k on the outboard side of the k th hinge is 

^ V(O k ) = V(Ot) + IT(k)0(k) (8) 

The spatial velocity V(k) of the k th body reference frame is given by 

V(k) = <f,-(O k ,k)V(O k )-u(d k ) 


= <t>*(o kl k)v(o k )-n d (k)i(k)( 9 ) 

Putting together Eq. (5), Eq. (7), Eq. (8) and Eq. (3.1), it follows that 

V(k) = Wk + l^Wik + V + Wh+ukWik + lWk+l) 

+P(O k ,k)H-(k)f3(k)~ U d (k)r,(k) 


(10) 
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Thus with Af(k) = n m (fc)+6, and using Eq. (10), the modal spatial velocity Kn(&) £ 
for the k th body is given by 

V«(t)= [v(l) )=*'( |1 + 1 ’ *)V«(i + l) + «*(i)x(i) € «*<*> (11) 

where the interbody transformation operator $(., .) and the modal joint map matrix 7i(k) 
are defined as 


where 


Note that 


where 


$(k 4- 1 k) = ( ^ [If (k + 1)] k) \ sp^(fc+i)x^(Jb) 

v ’ 1 \o <Kk + i,k) j 

H{k) = ( 1 ) € ^(*) X ^W 

0 

Hr(k) = H(k)<f>(O k , Jt)er ( ‘> x6 

$(* + l,A:) = .4(lfc + l)£(Jfc + !,*:) 


( 12 ) 

( 13 ) 


( 14 ) 


“ ( Sm!) ) e ^ WX6 and 5 (* + M) = [0, (15) 


Also, the modal joint map matrix H(k) can be partitioned as 


H(k) = ( H ) 6 
V ^r(k) j 


( 16 ) 


25 


where 


Kj(k) = [/, — [n d (Ar)]“] € R"">( fc > x ^(*) and H r {k) = [0, H(k)<f>{O k ,k)] € RM*)xaT(*) 

( 17 ) 

With W = 1 77 (k), we define the spatial operator £$ 


as 


30 


£<t = 


l o 0 0 0 0 \ 

$( 2 , 1 ) 0 ... 0 0 

0 $(3,2) ... 0 0 

^ 0 0 ... $(N,N — 1) 0 ) 


<^ST xJT 


( 18 ) 
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Using the fact that £$ is nilpotent (i.e. £$ = 0), we define the spatial operator $ as 


0 ... 0 




${N, 1) $(N, 2) ... I 




where 

$(i,j) = $(i,i - 1) • • • $(j + 1, j) for i > j 

Also define the spatial operator T~t = diag{H(A:)} G St***. Using these spatial operators, 

and defining V m = col|l4n(&)| G 3 , from Eq. (11) it follows that the spatial operator 
expression for V m is given by 

V m = $*H’x (20) 


20 


3.2 Modal Mass Matrix for a Single Body 

With V a (j k ) G 3? 6 denoting the spatial velocity of node j k , and V,(k ) = col (j G 

3?® ^ the vector of all nodal spatial velocities for the k th body, it follows (see Eq. (5)) 

that 

V a (k) = B'(k)V(k) + u(k) = [n(A:), Zr(*)]V m (*) (21) 

where 

B(k) = [^(A:,10,^(i,20r--,^,n 4 ()t))]G» 6x6n * w (22) 

Since M a (k) is the structural mass matrix oi the k th body, and using Eq. (21), the kinetic 
energy of the k tfl body can be written in the form 

\v;(k)M.(k)V.(k) = \v^(k)M m (k)V m (k) 

where 


30 


M m (k) ± 


( n-(*) 

{ B(k) 


M s (k)[U(k ), B'(k)] = 


( M"(k) MB(k) 

{ M r J(k) M”(k) 


€ ^(k)xjr(k) 


n-(k)M s (k)n(k) n-(k)M,(k)B-(k) \ 
B(k)M 3 (k)U(k) B{k)M a {k)B‘(k) j 

(23) 


Corresponding to the generalized speeds vector X (k), M m (k ) as defined above is the modal 
mass matrix of the k th body. In the block partitioning in Eq. (23), the superscripts / and 
r denote the flexible and rigid blocks respectively. Thus M*/(k) represents the flex/flex 
coupling block, while M^(k) the flex/rigid coupling block of M m (k). We will use this 
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notational convention through this specification. This partitioning is readily carried out 
by simply recognizing that the Mjj{k) block is a square matrix of dimensionality equal 
to the number of deformation modes while the M”(k) block is a square 6-by-6 matrix. 
Note that M”(k) is precisely the rigid body spatial inertia of the k th body. Indeed, 
5 M m (k) reduces to the rigid body spatial inertia when the body flexibility is ignored, i.e., 
no modes are used, since in this case n m (k ) = 0 (and II(fc) is null). 

Since the vector l(k,jk) from Tk to node jk depends on the deformation of the 
node, the operator B(k) is also deformation dependent. From Eq. (23) it follows that 
0 while the block ( k ) is deformation independent, both the blocks M^{k) and M”(k) 
are deformation dependent. The detailed expression for the modal mass matrix can 
be defined using modal integrals which are computed as a part of the finite-element 
structural analysis of the flexible bodies. These expressions for the modal integrals and 
the modal mass matrix of the k th body can be found in [10]. Often the deformation 
dependent parts of the modal mass matrix are ignored, and free-free eigen-modes are 
used for the assumed modes I1(A:). When this is the case, Mj^(k) is zero and M£/{k) is 
block diagonal. 

3.3 Recursive Propagation of Accelerations 
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Differentiating the velocity recursion equation, Eq. (11), we obtain the following recursive 
expression for the modal spatial acceleration a m (A:) £ for the k th body: 


Ctm(k) = V m (k) = 


m 

a(k) 


*•(* + 1, *)a m (Jfc + 1) + n-(k)x(k) + a m (k) (24) 


where ct{k) = V(k), 
given by 


and the Coriolis and centrifugal acceleration term a m (k) £ is 


.(*) = + + 1 ) + 


df 


dt 


(25) 


The detailed expressions for a m (k) can be found in [10]. Defining a m = col|a m (Jt)| £ ^ 
and a m = col ja m (fc)j £ and using spatial operators we can rexpress Eq. (24) in the 
form 


a m = <i>-(H-x + a m ) (26) 

The vector of spatial accelerations of all the nodes for the k th body, ct s (k) = col ja^j/t) j 
€ & 6n 'W, is obtained by differentiating Eq. (21): 


°.(k) = V t (k) = [!!(*), £-(*)]a m (*) + a(k) 


(27) 



where 


■(*) = col{o(j t )} = d|n( * l ^---- <r - l V' m («r) € »«"■'*> 


(28) 


3.4 Recursive Propagation of Forces 


Let f(k — 1) G 3ft 6 denote the effective spatial force of interaction, referred to frame 
Tk- i, between the k th and ( k — l) t/l bodies across the ( k — l) th hinge. Recall that the 
(k — l) </l hinge is between node t k on the k th body and node dk-\ on the ( k — l) tft body. 
With f,(jk) 6 3? 6 denoting the spatial force at a node jk, the force balance equation for 
node tk is given by 


Mh) = H*k,k-l)f(k-l) + M.(t k )a,{t k ) + b(t k ) + f K (t k ) (29) 


For all nodes other than node tk on the k th body, the force balance equation is of the 
form 

fs(jk) = M,(jk)a,(jk) + b(jk) + fh'(jk) (30) 

In the above, /*-(&) = K $ (k)u(k) 6 denotes the vector of spatial elastic strain 

forces for the nodes on the k th body, while b(jk) € 3ft 6 denotes the spatial gyroscopic force 
for node jk and is given by 




u(jk)j{jk)u(jk) \ € g> 
™(jk)u(jk)u>(jk)p(jk) ) 


(31) 


where u){j k ) 6 3R 3 denotes the angular velocity of node j k . Collecting together the above 
equations and defining 


C(k,k- 1 ) = 


( 0 \ 


<j>{t k ,k - 1 ) 




V 0 1 

it follows from Eq. (29) and Eq. (30) that 


/.(*) = C(k,k-l)f(k-l) + M,(k)a.(k) + b(k) + K.(k)u(k) (33) 

where j,(k) = col{/,(; t )} € Sf®’*'***. Noting that 


fW « B(k)j.(k) 


(34) 
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and using the principle of virtual work, it follows from Eq. (21) that the modal spatial 
forces f m (k) € for the k th body are given by 


«*> • 

n-(*) 


(35) 


Premultiplying Eq. (33) by I I and using Eq. (23), Eq. (27), and Eq. (35) leads 

B(k) 

to the following recursive relationship for the modal spatial forces: 

II*(fc)C(M-l) 


fm(k) = 


B(k)C{k,k- 1) 

[n‘(*)l* 


f(k - 1) + M m (k)a m (k) + b m (k) + K m (k)d(k) 
m , k - 1 )f(k - 1) + M m (k)a m (k) + b m (k) + K m (k)d(k) 


<t>{k,tk) 

= *(*, k - 1 )f m (k - 1) + M m (k)a m (k) + b m (k ) + K m (k)d(k) 


Here we have defined 


b m(k) = ( 1 m + M.(k)a(k)} e 


(36) 


(37) 


and the modal stiffness matrix 


K m (k) = ^ n*(A:)/if J ,(Ar)n(A:) 0 ^ ^ ^(k)xJT{k) 


(38) 


The expression for K m (k) in Eq. (38) uses the fact that the columns of B*(k) are indeed 
the deformation dependent rigid body modes for the k th body and hence they do not 
contribute to its elastic strain energy. Indeed, when a deformation dependent structural 
stiffness matrix K a (k ) is used, we have that 


K a (k)B-(k) = 0 


(39) 


However the common practice (also followed here) of using a constant, deformation- 
independent structural stiffness matrix leads to the anomalous situation wherein Eq. (39) 
does not hold exactly. We ignore these fictitious extra terms on the left-hand side of 
Eq. (39). 

The velocity -dependent bias term b m (k) is formed using modal integrals generated 
by standard finite- element programs, and a detailed expression for it is given in [10]. 
From Eq. (36), the operator expression for the modal spatial forces f m = col|/ m (Jt)| € 
3?^ for all the bodies in the chain is given by 


fm — $( A/ m a m + b m + A m t?) 


(40) 



where 


M m = diag{M m (A:)} € K m = diag{tf m (*)} € X*’**’, and b m = col{b m (k)} € 

From the principle of virtual work, the generalized forces vector T € for the multibody 

system is given by the expression 

T = Hf m (41) 


3.5 Operator Expression for the System Mass Matrix 

Collecting together the operator expressions in Eq. (20), Eq. (26), Eq. (40) and Eq. (41) 
we have: 

V m = VH'x 

a m = $*(7Y*x + a m ) (42) 

fm = ^M m a m + b m + I< m ti) = $M m $-H m x + ${M m $-a m + b m + K m i)) 

T = Hf m = + b m ) 

= Mx + C 

where 

M = H$M m W e and C = W{M m Va m + b m + K m d) € (43) 

Here A4 is the system mass matrix for the serial chain and the expression 

is referred to as the Newton-Euler Operator Factorization of the mass matrix. C is the 

vector of Coriolis, centrifugal, and elastic forces for the system. 

It is noteworthy that the operator expressions for M and C are identical in form 
to those for rigid multibody systems (see [1, 11]). Indeed, the similarity is more than 
superficial, and the key properties of the spatial operators that are used in the analysis 
and algorithm development for rigid multibody systems also hold for the spatial operators 
defined here. As a consequence, a large part of the analysis and algorithms for rigid 
multibody systems can be easily carried over and applied to flexible multibody systems. 
This is the approach adopted here. 

4. Inverse Dynamics Algorithm 


This section describes a recursive Newton-Euler inverse dynamics algorithm for comput- 
ing the generalized forces 7, for a given set of generalized accelerations x And system 
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state The inverse dynamics algorithm also forms a part of forward dynamics 

algorithms such as those based upon composite body inertias or the conjugate gradient 
method ( [12]). 


Collecting together the recursive equations in Eq. (11), Eq. (24), Eq. (36) and 
Eq. (41) we obtain the following recursive Newton-Euler inverse dynamics algorithm: 

V m (N + 1) = 0, a m (N + 1) = 0 
for k = N • • • 1 

v m (k) = $-(k + i,k)v m (k + \) + n-(k) x (k) 

a m {k) = $*(£ + 1, k)a m (k + 1) + H m (k)x(k) + a m (k) 

end loop 

(44) 

fm( 0 ) = 0 
for k = 1 • • • N 

/*(*) = Hk,k-l)f m (k-l) + M m (k)a m (k) + b m (k) + K m (k)d(k) 

T(k ) = H(k)f m (k) 

end loop 

The structure of this algorithm closely resembles the recursive Newton-Euler inverse 
dynamics algorithm for rigid multibody systems (see [13, 1]). All external forces on 
the k th body are handled by absorbing them into the gyroscopic force term b m (k). Base 
20 mobility is handled by attaching an additional 6 degree of freedom hinge between the 

mobile base and an inertial frame. 
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By taking advantage of the special structure of $(k + 1, k ) and H(k) in Eq. (12) 
and Eq. (13), the Newton-Euler recursions in Eq. (44) can be further simplified. Using 
block partitioning and the superscripts / and r as before to denote the flexible and rigid 
components or versions of the various quantities, we have that 




and T(k) = 


( T'(k) \ 
\ T r {k) ) 
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It is easy to verify that Eq. (45) below is a simplified version of the inverse dy- 
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namics algorithm in Eq. (44). 


< 


< 


V m (N + 1) = 

0, a m (N + 1) = 0 



for k = N- 

• 1 




V'(t) = 

r)(k) 




KW = 

<t> m (tk+ u 

t k)A*(k + l)V m (k + 1) + 

//>(*)/?(*) - 

- Ti-wm 

II 

S' 

^ £ 
S 

m 




Q m(*0 = 


k)A*(k + l)a m (fc +1)4* 

Hmm - 

- n mm + «*) 

end loop 






( 45 ) 


/m(0) = 0 

for k = 1 • • • N 

fm(k) = >l(A:)^ /t ,fc-l)/;(A : -l) + M m (fc)a m (fc) + 6 m (A:) + ^ m (A:)tf(it) 
T<k) = I TJ{k) ) = I /«<*) - PW-fcW \ 

{ r(k) ) [ H T {k)r m {k) ) 

end loop 


In the foregoing algorithm, f)(k) and rj(k) are the modal deformation velocities and accel- 
erations, respectively, computed from the results obtained for a previous time step by a 
forward dynamics algorithm of the type described below herein. Flexible multibody sys- 
tems have actuators typically only at the hinges. Thus for the k ih body, only the subset 
of the generalized forces vector T(k) corresponding to the hinge actuator forces T r ( k ) can 
be set, while the remaining generalized forces T*(k) are zero. Thus in contrast with rigid 
multibody systems, flexible multibody systems are under-actuated systems ( [14]), since 
the number of available actuators is less than the number of motion degrees of freedom in 
the system. For such under-actuated systems, the inverse dynamics computations for the 
generalized force T are meaningful only when the prescribed generalized accelerations \ 
form a consistent data set. For a consistent set of generalized accelerations, the inverse 
dynamics computations will lead to a generalized force vector T such that T f ( .) = 0. 


5. Composite Body Forward Dynamics Algorithm 

30 

The forward dynamics problem for a multibody system requires computing the gener- 
alized accelerations \ for a given vector of generalized forces T and state of the system 
The composite body forward dynamics algorithm described below consists of the 
following steps: (a) computing the system mass matrix M , (b) computing the bias vector 
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C, and (c) numerically solving the following linear matrix equation for x: 

Mx = T-C (46) 

Later in Section 6 we describe the recursive articulated body forward dynamics algorithm 
5 that does not require the explicit computation of either At or C. 

It is evident from Eq. (46) that the components of the vector C are the generalized 
forces for the system when the generalized accelerations x are all zero. Thus C can be 
computed using the inverse dynamics algorithm in Eq. (45). We describe next an efficient 
q composite-body-based recursive algorithm for the computation of the mass matrix M. 
This algorithm is based upon the following lemma which contains a decomposition of 
the mass matrix into block diagonal, block upper triangular and block lower triangular 
components. 
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Lemma 5.1: Define the composite body inertias R(k) £ 9 ^(k)xAT(k) recurs j ve ]y f or 

all the bodies in the serial chain as follows: 

R{ 0) = 0 
for k = 1 • • • N 

R{k) = ${k,k-\)R(k-l)<ir(k,k-l) + M m (k) ^ 

end loop 

Also define R = diagj.ft(fc)j £ Then we have the following spatial operator 

decomposition 
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where $ = $ 




= R + QR + RQ* 


(48) 


Proof: See Appendix A. 


Physically, R{k) is the modal mass matrix of the composite body formed from 
all the bodies outboard of the k th hinge by freezing all their (deformation plus hinge) 
degrees of freedom. It follows from Eq. (43) and Lemma 5.1 that 

M = = HRH * + H$RH' + HR&H' (49) 

Note that the three terms on the right of Eq. (49) are block diagonal, block lower tri- 
angular and block upper triangular respectively. The following algorithm for computing 
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the mass matrix M. computes the elements of these terms recursively: 


72(0) = 0 
for k = 1 • • • N 


m 

X(k) 

M„(k,k) 


$(k,k-l)R(k-l)<tr(k,k-l) + M m (k) 

A{k)<f>(t k , k - 1 )R rr (k - l)<f> m (t k , k - l)A'(k) + M m (k) 
R(k)H m {k) 

H(k)X(k) 

r 


for j = (k+l)---N 

X(j) = $(j,j — l)X(j — 1 ) = A(j)<j>(tj,j — l)X r (j — 1 ) 
M(j,k) = M*(k,j) = H(j)X(j) 

end loop 

_ end loop 


( 50 ) 

The main recursion proceeds from tip to base, and computes the blocks along the di- 
agonal of Ad. As each such diagonal element is computed, a new recursion to compute 
the off-diagonal elements is spawned. The structure of this algorithm closely resembles 
the composite rigid body algorithm for computing the mass matrix of rigid multibody 
systems ( [12, 8]). Like the latter, it is also highly efficient. Additional computational 
simplifications of the algorithm arising from the sparsity of both 'Hj(k) and 'H-r(k) are 
easy to incorporate. 


6. Factorization and Inversion of the Mass Matrix 


An operator factorization of the system mass matrix Ad, denoted the Innovations Oper- 
ator Factorization, is derived in this section. This factorization is an alternative to the 
2S 

Newton-Euler factorization in Eq. (43) and, in contrast with the latter, the factors in the 
Innovations factorization are square and invertible. Operator expressions for the inverse 
of these factors are developed and these immediately lead to an operator expression for 
the inverse of the mass matrix. The operator factorization and inversion results here 

closely resemble the corresponding results for rigid multibody systems (see [1]). 

30 

Given below is a recursive algorithm illustrated in Figure 2 which defines some 
required articulated body quantities. In the following algorithm, P(k) is the articulated 
body inertia of body k, D(k) is the articulated hinge inertia of hinge k, £?(&) is a body 
to hinge force operator of body and hinge k, and r(k) is a null force operator for hinge 
k which accounts for the component of applied force resulting in no hinge acceleration. 
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P + (0) = 0 

for k = 1 • • • N 

P(k) = Q(k,k-l)P+(k-l)9*(k,k-l) + M m (k)e& r W* Jr W 

D(k) = H(k)P(k)H'(k) e 

G(k) = P(k)H m (k)D~ l (k) € S^<*> x ^<*> 

/r(Jfe + l,Jt) = $(Ar+l,Jfc)G(Jfc)€3^ ( * )x ^ (fc) 

r(jfc) = I -G{k)H(k) 

P+(k) = r(k)P(k) e 
9(k + l,k) = $(*: + 1, k)T(k) 6 3R^(*)*^( fc ) 

end loop 

(51) 

The operator P € is defined as a block diagonal matrix with the k th diagonal 

element being P(k). The quantities defined in Eq. (51) form the component elements of 
the following spatial operators: 

D = HPH' = diag{D(ifc)} 6 
G = PH'D- 1 = diag{G(Jt)} € 

K = S*G € 

r = I - GH = diag{r(Jfc)} <E 3p x ^ 

£* = £ 4 tg^ x7 (52) 

The only nonzero block elements of K and £* are the elements’ K(k + 1, Jfc)’s and V(k + 
l,A:)’s respectively along the first sub-diagonal. 

As in the case for £$, S\ y is nilpotent, so we can define the operator 'P as follows: 


where 


9 = {!-€*)-' = 


9(2,1) 


9(N,1) V(N, 2) 




9(i,j) = «(*,*- 1) ••• 9(j + l,j) for i>j 


The structure of the operators and is identical to that of the operators £* and $ 
respectively except that the component elements are now <P(i, j) rather than Also, 

the elements of have the same semigroup properties as the elements of the operator 
and as a consequence, high-level operator expressions involving them can be directly 
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mapped into recursive algorithms, and the explicit computation of the elements of the 
operator 'I' is not required. 

The Innovations Operator Factorization of the mass matrix is defined in the following 
lemma. 

Lemma 6.1: 

M = [I + H*K]D[I + H*KY (54) 

Proof: See Appendix A. | 

Note that the factor [7 + T~l$K] £ j s S q Uar e 5 block lower triangular and 

nonsingular, while D is a block diagonal matrix. This factorization provides a closed- 
form expression for the block LDL m decomposition of A4. The following lemma gives the 
closed form operator expression for the inverse of the factor [7 -j- Ti^K]. 


20 


Lemma 6.2: 

[7 + HQK]- 1 = [7 - H$A'] 


Proof: See Appendix A. 


(55) 

I 


It follows from Lemmas 6.1 and 6.2 that the operator expression for the inverse of the 
mass matrix is given by: 

25 

Lemma 6.3: 

M~ x = [7 - 7 iVK)*D- l [I - HVK] (56) 

I 

30 

Once again, note that the factor [7 — 7f\kA] is square, block lower triangular and 
nonsingular and so Lemma 6.3 provides a closed-form expression for the block LDL ’ 
decomposition of .M -1 . 
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7. Articulated Body Forward Dynamics Algorithm 


We first use the operator expression for the mass matrix inverse developed in Section 6 to 
obtain an operator expression for the generalized accelerations This expression directly 
5 leads to a recursive algorithm for the forward dynamics of the system. The structure of 
this algorithm is completely identical in form to the articulated body algorithm for serial 
rigid multibody systems. The computational cost of this algorithm is further reduced 
by separately processing the flexible and hinge degrees of freedom at each step in the 
recursion, and this leads to the articulated body forward dynamics algorithm for serial 
10 flexible multibody systems. This algorithm is an alternative to the composite-body 
forward dynamics algorithm developed earlier. 

The following lemma describes the operator expression for the generalized accel- 
erations \ in terms of the generalized forces T. 


15 


Lemma 7.1: 


X = [I- H*K]-D - 1 [T - m{KT + Pa m + b m + - K"Ta m (57) 


20 


Proof: See Appendix A. 


■ 


As in the case of rigid multibody systems ( [1, 2]), the direct recursive implemen- 
tation of Eq. (57) leads to the following recursive forward dynamics algorithm illustrated 
in Figure 3. In the following algorithm, z(k ) is a residual body force on body k, e(k) is 
25 the resultant hinge force on hinge k, u(k) is the resultant hinge acceleration of hinge k 

and z + (&) is the revised residual body force on body k: 

2 +( 0)=0 

for k = 1 • • • n 

z(k) = *(k,k-l)z + (k-l) + P(k)a m (k) + b m (k) + K m (k)#(k) 

30 1 < k ) = T(k) - H(k)z(k) 

u(k) = D-\k)e(k) 
z + (k) = z(k) + G(k)e{k) 

end loop 


( 58 ) 
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a m (n + 1 ) = 0 

for k = n • • • 1 

a+(k) = P(k + l,k)a m (k + l) 

X(k) = *(*) - G?(t)a+(k) 

5 <*m(k) = °m(k) + ft‘(k)x(k) + a TO (fc) 

L end loop 

The structure of this algorithm is closely related to the structure of the well known 
Kalman filtering and smoothing algorithms ( [8]). All the degrees of freedom for each 
body (as characterized by its joint map matrix H*(.)) are processed together at each 
10 recursion step in this algorithm. However, by taking advantage of the sparsity and 
special structure of the joint map matrix, additional reduction in computational cost is 
obtained by processing the flexible degrees of freedom and the hinge degrees of freedom 
separately. These simplifications are described in the following sections. 

71 Simplified Algorithm for the Articulated Body Quantities 

Instead of a detailed derivation, we describe here the conceptual basis for the separation 
of the modal and hinge degrees of freedom for each body. First we recall the velocity 
recursion equation in Eq. (11) 


20 
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v m (k) = *•(* + 1 , k)V m (k + 1 ) + n m (k)x(k) ( 59 ) 

and the partitioned form of H(k) in Eq. (13) 

n(k) - ( *$ ) < 6 °> 
Introducing a dummy variable k', we can rewrite Eq. (59) as 

V m (k') = $ m (k+l,k')V m (k + l) + H}(k)ri(k) 

v m (k) = V(k’,k)V m (k') + H;(k)0(k ) ( 61 ) 

where 

*(k + l,k') = $(k + l,k) and $(*',*) = / 

Conceptually, each flexible body is now associated with two new bodies. The first one 
has the same kinematical and mass/inertia properties as the real body and is associated 
with the flexible degrees of freedom. The second body is a fictitious body and is massless 
and has zero extent. It is associated with the hinge degrees of freedom. The serial chain 
now contains twice the number of bodies as the original one, with half the new bodies 
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being fictitious ones. The new 7i * operator now has the same number of columns but 
twice the number of rows as the original 'H“ operator. The new $ operator has twice 
as many rows and columns as the original one. Repeating the analysis described in the 
previous sections, we once again obtain the same operator expression as Eq. (57). This 
5 expression also leads to a recursive forward dynamics algorithm as in Eq. (58). However 
each sweep in the algorithm now contains twice as many steps as the original algorithm. 
But since each step now processes only a smaller number of degrees of freedom, this 
leads to a reduction in the overall cost. In the following algorithm, the subscript r 
denotes the rigid component or version of the subscripted quantity while the subscript 
10 f denotes the flexible component or version of the subscripted quantity. Thus, 'Hj(k) is 
a matrix including the corresponding modal spatial influence vector, while 'H. r (k) is a 
matrix including the corresponding transformed joint map matrix. The new algorithm 
(replacing Eq. (51)) for computing the articulated body quantities is as follows: 


15 
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30 


p*( 0) 

= 

0 

for k = 1 • • • 

N 


r(*) 

= 

B(k, k - I)p + (k - 1 )£*(£, k - 1 ) e X 6 * 6 

p(k) 

= 

A(k)T(k)A m {k) + M m (k) e %p( k )*X( k ) 

Dj{k) 

= 

Hj{k)P{k)H){k) <= £"-<*) xn -(*> 

Gj{k) 

= 

P{k)H){k)D]\k) e »^<*> Xn ”*W 

Tj{k) 

— 

i - G f (k)Hf(k) e 

Pr(k) 

= 

Tf(k)P(k) € 

D r (k) 

= 

n r {k)p r {k)H;{k) € 

G r (k) 

= 

p r {k)n;{k)D;\k) € s^w x *-w 

T r (k) 

= 

I - G r (k)H T {k) e ^ (k)x77 ^ 

P+(k) 

= 

T r {k)P r (k ) € ti*( k )*X( k ) 

«(* + !,*) 

= 

$(k -f 1, k)r(k) 6 


end loop 


We now use the sparsity of B(k + l,fc), 'Hj(k) and 7i r (k) to further simplify the above 
algorithm. Using the symbol “x" to indicate “don’t care” blocks, the structure in block 
partitioned form of some of the quantities in Eq. (62) is given below. In the following 
algorithm, the subscripts f and r have the same significance as that discussed previously 
herein, the subscript R denotes another rigid version of the subscripted quantity (defined 
below), while P rf (k) and P rr (k) denote the blocks of the articulated body inertia P(k) 
partitioned in the same manner as that discussed previously herein with reference to the 
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partitioning of the modal mass matrix in Equation (23): 


5 


15 


r(fc) = k — l)P£(k — k — 1), (^r(^) is defined below) 

Gj{k) = ( ^ ) . where g(k) = n{k)D] l (k) G 


and n(k) = [P Tl {k), P rr {k))H){k) G » 6xn ”* ( * ) 

Pr (k) = p * k) j , where P R (k) = P”{k) - g{k)^{k) € ^ 

D r {k) = 6 £ nr( * )xn ' W 

Gr(k) = ^ j , where G R (k) = P R {k)H^{k)D;\k) G » 6xn '<*> 

^r(fc) = ^ q r *y J , where T R (k) = / - G«(A:)^(it) G » 6x6 


P + (k) 



where P R (k) = r R (k)P R (k) G §f 6x6 


Using the structure described above, the simplified algorithm for computing the articu- 
lated body quantities is as follows: 


25 
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P* + (0) = 0 
for k = 1 • • • N 

F(*) = *(<*,* -l)Pjt(Jt-l)^(* fc ,*-l) 

P(k) = A(k)T(k)A'(k) + M m (k) 

D f (k) = H,(k)P(k)H}(k) 

M*) = [P T f(k), P rr (k)]7i}(k) 

g(k) = fi(k)Dj 1 (k) (63) 

P R (k) = P”(k)-g(k)f(k) 

D R (k) = H?{k)P R {k)H}r{k) 

G R {k) = P R {k)H* T (k)Dl\k) 

T R (k) = I-G R (k)Hr(k) 

P£(k) = r R (k)P R (k) 
end loop 
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7.2 


Simplified Articulated Body Forward Dynamics Algorithm 


5 


10 


15 
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The complete recursive articulated body forward dynamics algorithm for a serial flexible 
multibody system follows directly from the recursive implementation of the expression in 
Eq. (57). The algorithm consists of the following steps as illustrated in Figure 4: (a) a 
base-to-tip recursion as in Eq. (45) for computing the modal spatial velocities V m (k) and 
the Coriolis and gyroscopic terms a m (k) and b m (k) for all the bodies; (b) computation 
of the articulated body quantities using Eq. (78) and Eq. (63); and (c) a tip-to-base 
recursion followed by a base-to-tip recursion for the joint accelerations y as described 
below and illustrated in Figures 5a and 5b: 


4(0) = o 

for k = 1 • • ■ N 



= A(k)<t>(t k , k - 1 )4(* - 1) + b m {k ) + K m (k)ti{k) € 
tj(k) = T J (k)-z J (k) + [n d (k)]'z r (k)eX n ”'W 
< i 'j(k) = Dj\k)e f {k) E K"-W 


z R {k) = z T {k) + g{k)t f (k) + P R (k)a mR (k ) E 
t R (k) = T r (k) - Hjr(k)z R (k) E 9? nr(fc) 
v R {k) = D^{k)t R {k) E 
z R (k) = z R (k) -f G R (k)t R (k) E 9ft 6 

end loop 


a m {N + 1) = 0 


for k = N • • • 1 

a R (k) 

m 

& R {k) 

m 

&m i.k'} 

end loop 


^*(4 +1 ,*)A*(fc+l)a m (^ + l)€» 6 

v R {k) - G' R (k)a+(k) E 9 ft"'<*) 

“£(*) + k)0(k) + a mR (k) E 3ft 6 

Vf(k) - g’(k)a R (k) E 9ft nm(fc) 

( m \ 

{ a n (k) - U d (k)rj(k) ) 


E 9ft^*> 


(64) 


The recursion in Eq. (64) is obtained by simplifying the recursions in Eq. (58) in the same 
manner as described in the previous section for the articulated body quantities. The rigid 
Coriolis and centrigugal acceleration a mR (k) is given in Appendix C below herein. 


In contrast with the composite body forward dynamics algorithm described in 



Section 5, the articulated body forward dynamics algorithm does not require the explicit 
computation of either M. or C. The structure of this articulated body algorithm closely 
resembles the recursive articulated body forward dynamics algorithm for rigid multibody 
systems described in references ( [15, 1]). 

The articulated body forward dynamics algorithm has been used to develop a 
dynamics simulation software package (called DARTS) for the high-speed, real-time, 
hardware-in-the-loop simulation capability for planetary spacecraft. Validation of the 
DARTS software was carried out by comparing simulation results with those from a stan- 
dard flexible multibody simulation package ( [6]). The results from the two independent 
simulations have shown complete agreement. 

A System Embodying the Invention: 

Referring to Figure 6, a robot manipulator 100 having flexible links (bodies), such 
as the manipulator illustrated in Figures la and lb, includes joint servos 110 controlling 
respective articulating hinges of the manipulator. A robot control computer 120 includes 
a processor 125 which computes the articulated body quantities of the manipulator 100 
from the current state of the manipulator 100 using the process of Figure 2. The current 
state of the manipulator 100 is also used by a processor 130 to compute the Coriolis 
and centrifugal accelerations and gyroscopic forces of the manipulator links using the 
algorithm of Equation (44). A set of link (body) forces is specified to a processor 135. The 
processor 135 uses the specified body forces, the articulated body quantities computed by 
the processor 125 and the gyroscopic and Coriolis terms computed by the processor 130 
to compute the deformation acceleration of the finite element nodes of each link (body) 
and the acceleration of each hinge by executing the algorithm of Figures 5a and 5b. 

In one embodiment of the invention described above with reference to Figure 4, 
the procesor 135 repeats its operation over successive time steps, and the configuration 
vectors of the manipulator 100 required by the processors 125 and 130 are computed by 
a processor 140 from the accelerations computed by the processor 135 for the previous 
time step. In an alternative embodiment of the invention, the hinge configuration vectors 
are derived by the processors 125 and 130 directly from joint sensors 142 on the hinges 
of the manipulator 100. 

As one example of the application of the results computed by the processor 135, 
a desired robot motion is defined by a set of user-specified node deformations and hinge 
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accelerations for a succession of time steps. The node deformations and hinge accelera- 
tions computed during each time step by the processor 135 are compared by a processor 
144 with a desired user-specified node deformations and hinge accelerations for the cor- 
responding time step to determine an error and to correct the specified body forces to 
5 reduce the error using well-known feedback control techniques. Such feedback control 
techniques are well- understood in the art and need not be described herein. The cor- 
rected body forces are then stored for later (or immediate) conversion by a processor 146 
to joint servo commands for transmittal to the joint servos 110. 

10 8. Computational Cost 

This section discusses the computational cost of the composite body and the articulated 
body forward dynamics algorithms. For low-spin multibody systems, it has been sug- 
gested in [16] that using ruthlessly linearized models for each flexible body can lead to 
15 significant computational reduction without sacrificing fidelity. These linearized models 
are considerably less complex and do not require much of the modal integral data for 
the individual flexible bodies. All computational costs given below are based on the 
use of ruthlessly linearized models and the computationally simplified steps described in 
Appendix B. 

20 

Flexible multibody systems typically involve both rigid and flexible bodies and, 

in addition, different sets of modes are used to model the flexibility of each body. As 

a consequence, where possible, we describe the contribution of a typical (non-extremal) 

flexible body, denoted the k th body, to the overall computational cost. Note that the 

computational cost for extremal bodies as well as for rigid bodies is lower than that for 
>5 

a non— extremal flexible body. Summing up this cost for all the bodies' in the system 
gives a figure close to the true computational cost for the algorithm. Without any loss in 
generality, we have assumed here that all the hinges are single degree of freedom rotary 
joints and that free-free assumed modes are being used. The computational costs are 
given in the form of polynomial expressions for the number of floating point operations 
with the symbol M denoting multiplications and A denoting additions. 


30 



36 


8.1 Computational Cost of the Composite Body Forward Dynamics Algo- 
rithm 


5 


The composite body forward dynamics algorithm described in Section 5 is based on 
solving the linear matrix equation 

Mx—T-C 


The computational cost of this forward dynamics algorithm is given below: 


10 


15 
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1. Cost of computing R(k) for the k ih body using the algorithm in Eq. (50) is 
[48 n m (k) + 90] M + (n^(A:) + fn m (fc) + 116]A. 

2. Contribution of the k th body to the cost of computing Ad (excluding cost of R(k)'s) 
using the algorithm in Eq. (50) is 

{A:[12n^(A:) + 34n m (&) + 13]} M + {fcfllnj^A:) + 24n m (fc) + 13]} A. 

3. Setting the generalized accelerations x = 0, the vector C can be obtained by using 
the inverse dynamics algorithm described in Eq. (45) for computing the generalized 
forces T. The contribution of the k th body to the computational cost for C(k) is 
{2 n 1 2 m {k) + 54n m (A:) + 206} M + {2 n 2 m (k) + 50 n m (fc) + 143} A. 

4. The cost of computing T — C is {A7} A. 

5. The cost of solving the linear equation in Eq. (46) for the accelerations x is 
{|Af 3 + | tf 2 - fAf} M + {|AT 3 + AT 2 - |AT} A. 


The overall complexity of the composite body forward dynamics algorithm is 0(A/" 3 ). 

25 

8.2 Computational Cost of the Articulated Body Forward Dynamics Algo- 
rithm 

The articulated body forward dynamics algorithm is based on the recursions described 

in Eq. (78), Eq. (63) and Eq. (64). Since the computations in Eq. (78) can be carried out 
30 

prior to the dynamics simulation, the cost of this recursion is not included in the cost of 
the overall forward dynamics algorithm described below: 

1. The algorithm for the computation of the articulated body quantities is given in 

Eq. (63). The step involving the computation of D _1 (/:) can be carried out either 



by an explicit inversion of D(k ) with 0(n^ ^k)) cost, or by the indirect procedure 
described in Eq. (63) with 0(n^ n {k)) cost. The first method is more efficient than 
the second one for n m (k ) < 7. 

• Cost of Eq. (63) for the k th body based on the explicit inversion of D(k) (used 
when n m (k ) < 7) is 

{!<(*) + fnl(k) + n m (k) + ISO} M+{|>4(*) + f «i(t) + *?n m (k) + 164} 

• Cost of Eq. (63) for the k th body based on the indirect computation of D~'(k) 
(used when n m (k) > 8) is {12 n^(fc) + 255n m (fc) + 572} M+{13n^(fc) + 182n m (/;) 

2. The cost for the tip-to-base recursion sweep in Eq. (64) for the k th body is 
{ n m(*0 + 25 n m (k) + 49} M + {n^k) + 24 n m (k) + 50} A. 

3. The cost for the base-to-tip recursion sweep in Eq. (64) for the k th body is {18n m (fc) + 52 
{\9n m (k) + 42} A. 

The overall complexity of this algorithm is O(Nn^), where n m is an upper bound on the 
number of modes per body in the system. 

From a comparison of the computational costs, it is clear that the articulated body 
algorithm is more efficient than the composite body algorithm as the number modes and 
bodies in the multibody system increases. The articulated body algorithm is faster by 
over a factor of 3 for 5 modes per body, and by over a factor of 7 for the case of 10 modes 
per body. The divergence between the costs for the two algorithms becomes even more 
rapid as the number of bodies is increased. 

9. Extensions to General Topology Flexible Multibody Sys- 
tems 

For rigid multibody systems, [11] describes the extensions to the dynamics formulation 
and algorithms that are required as the topology of the system goes from a serial chain 
topology, to a tree topology and finally to a closed-chain topology system. The key to 
this progression is the invariance of the operator description of the system dynamics to 
increases in the topological complexity of the system. Indeed, as seen here, the operator 
description of the dynamics remains the same even when the multibody system contains 
flexible rather than rigid component bodies. Thus, using the approach in [11] for rigid 
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multibody systems, the dynamics formulation and algorithms for flexible multibody sys- 
tems with serial topology can be extended in a straightforward manner to systems with 
tree or closed-chain topology. Based on these observations, extending the serial chain 
dynamics algorithms described in this specification to tree topology flexible multibody 
5 systems requires the following steps: 

1. For each outward sweep involving a base to tip(s) recursion, at each body, the 
outward recursion must be continued along each outgoing branch emanating from 
the current body. 

10 

2. For each inward sweep involving a tip(s) to base recursion, 

at each body, the recursion must be continued inwards only after summing up 
contributions from each of the other incoming branches for the body. 

15 A closed-chain topology flexible multibody system can be regarded as a tree topol- 

°gy system with additional closure constraints. As described in [11], the dynamics algo- 
rithm for closed-chain systems consists of recursions involving the dynamics of the tree 
topology system, and in addition the computation of the closure constraint forces. The 
computation of the constraint forces requires the effective inertia of the tree topology 

2 Q system reflected to the points of closure. The 

algorithm for closed— chain flexible multibody systems for computing these inertias 
is identical in form to the recursive algorithm described in [11], 

10. Conclusions 

25 

This invention uses spatial operator methods to develop a new dynamics formulation for 
flexible multibody systems. A key feature of the formulation is that the operator descrip- 
tion of the flexible system dynamics is identical in form to the corresponding operator 
description of the dynamics of rigid multibody systems. A significant advantage of this 

30 unifying approach is that it allows ideas and techniques for rigid multibody systems to be 

easily applied to flexible multibody systems. The Newton-Euler Operator Factorization 
of the mass matrix forms the basis for recursive algorithms such as those for the inverse 
dynamics, the computation of the mass matrix, and the composite body forward dynam- 
ics algorithm for the flexible multibody system. Subsequently, we develop the articulated 
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body forward dynamics algorithm, which, in contrast to the composite body forward dy- 
namics algorithm, does not require the explicit computation of the mass matrix. While 
the computational cost of the algorithms depends on factors such as the topology and the 
amount of flexibility in the multibody system, in general, the articulated body forward 
5 dynamics algorithm is by far the more efficient algorithm for flexible multibody systems 
containing even a small number of flexible bodies. All of the algorithms are closely related 
to those encountered in the domain of Kalman filtering and smoothing. While the major 
focus in this specification is on flexible multibody systems with serial chain topology, the 
extensions to tree and closed chain topologies are straightforward and are described as 
10 well. 

While the invention has been described in detail by specific reference to preferred 
embodiments thereof, it is understood that variations and modifications may be made 
without departing from the true spirit of the invention. 
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Appendix A: Proofs of the Lemmas 

At the operator level, the proofs of the lemmas in this publication are completely analo- 
gous to those for rigid multibody systems ( [1, 2]). 

5 

Proof of Lemma 5.1: Using operators, we can rewrite Eq. (47) in the form 

M m = R- E*REl (65) 

From Eq. (19) it follows that $>£$ = £#$ = $ — / = $. Multiplying Eq. (65) from the 
10 left and right by $ and 3>* respectively leads to 

= ( 4 > + I)R($ + /)* - = R + $R + R$ m 

■ 


15 


20 


Proof of Lemma 6.1: It is easy to verify that tPt* = tP. As a consequence, the 
recursion for P(.) in Eq. (51) can be rewritten in the form 

M m = P-E^PEl = P-£^PE; = P-E^PE; + KDK m (66) 

Pre- and post-multiplying the above by $> and respectively then leads to 

= P + $P + Pfr + QKDK’Q* 


Hence, 


=> M = WA/ m r?f = n[P + $P+ pi>* + $KDK m $ m ]H m 
25 = D + H<t>I\D + + HQKDICVH' = [/ + H$K}D\I + H$K\ m 


Proof of Lemma 6.2: Using a standard matrix identity we have that 
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[1 + H9K]- 1 = I- H$[I + KH$]-'K 

(67) 


Note that 





4 ' -1 = /-£* = (/ - £*) + E+GH = + KH 

(68) 


from which it follows that 

= / + KH* 
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Using this with Eq. (67) it follows that 

[I + WK]-' = / - H*['ir 1 *)- 1 K = I - HVK 


5 


■ 


Proof of Lemma 7.1: From Eq. (42) and Eq. (43), the expression for the generalized 
accelerations \ is given by 
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X = M- l {T-C) = lI-H*K} m D- l [I-'H*K][T-M[M m *-a m + b m + K m 0]](Q9) 


From Eq. (68) we have that 

[/ - mK)H* = - KH\$ = H* 

Thus Eq. (69) can be written as 

X = [/ - H*K)-D- 1 [T - m[KT + M m Va m + 6 ro + 
From Eq. (66) it follows that 


(70) 

(71) 


M m = P-£*PS; =* = VP + (72) 

and so Eq. (71) simplifies to 

X = [I - HVKYD-' [: T - H*[KT + Pa m + 6 m + K m d\ - HPVa m j (73) 

From Eq. (68) we have that 

[/ - K\' D- l HP^‘ = [I - H*K) m K'V = - KH]*$’ = iT#* (74) 

Using this in Eq. (73) leads to the result. I 
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Appendix B: Ruthless Linearization of Flexible Body Dynam- 

ics 
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10 
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It has been pointed out in recent literature ( [17, 16]) that the use of modes for modeling 
body flexibility leads to “premature linearization” of the dynamics, in the sense that 
while the dynamics model contains deformation dependent terms, the geometric stiffening 
terms are missing. These missing geometric stiffening terms are the dominant terms 
among the first-order (deformation) dependent terms. In general, it is necessary to take 
additional steps to recover the missing geometric stiffness terms to obtain a “consistently” 
linearized model with the proper degree of fidelity. However for systems with low spin 
rate, there is typically little loss in model fidelity when the deformation and deformation 
rate dependent terms are dropped altogether from the dynamical equations of motion ( 
[16]). Such models have been dubbed the ruthlessly linearized models. These linearized 
models are considerably less complex, and do not require most of the modal integrals 
data for each individual flexible body. In this model, the approximations to M m (k), 
a m (k), and b m (k) are as follows: 


M m (k) * M° (*), a m (k) 



and b m (k) sa 6° (k) 


(75) 


With this approximation, M m (k) is constant in the body frame, while a m (fc) and b m (k) 
are independent of 77 (k) and fi(k). With this being the case, the formation of D~ l in 
Eq. (51) can be simplified. Using the matrix identity 

[A + BCB m }~ 1 = A- 1 - A-'BIC- 1 + B’ A~ l B]~' B* A~ l (76) 


which holds for general matrices A , B and C, it is easy to verify that 


Dj\k) = A(Jfc) - T(fc)[r -1 (fc) + n(*)] _1 (lfc)T*(*) (77) 

where the matrices A (k), f l(k), and T (k) are precomputed just once prior to the dynam- 
ical simulation as follows: 


for k = 1 ■ • • N 

A(Ar) = [H f (k)M m (k)'H}(k)}-' e ft"*" 

< C(jfc) = Hj{k)A{k) € »^ x6 (78) 

T (*) = A(k)((k) € 3fJ Arx6 
U(k) = (*(k)T(k) G 3f 6x6 

end loop 

Using Eq. (77) reduces the computational cost for computing the articulated body inertias 
to a quadratic rather than a cubic function of the number of modes. 
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Appendix C: Expressions for M m (k ), a m (k ) and b m (k) 


The modal spatial displacement influence vector IF(fc) for node jk has the structure: 


n-'(ib) = I ) 6 » 6xn *” W 

1 7 3 {k) I 


The components of the vectors A J (fc) £ gj3xn m (k) € 3fJ 3XT » m (*) are the modal slope 

displacement influence vector and the modal translational displacement influence vector 
respectively for node jk . They define the contribution of the various modes to the slope 
(or differential change in orientation) and translational deformation for the jj. h node on 
the k tfl body. Define 

A(ifc) = ^(k)rj{k) € 3fP, S v (jk) = 7 3 (k)v(k) € B 3 , and = 7 J (*)*?(fc) € ft 3 (80) 


Note that 


K^ijk) — lo(jk) + ^l(jit) 


where /o(j)t) denotes the undeformed vector from frame Tk to node jk- Note that M,(jk ) 
denotes the spatial inertiaof the j th node on the k th body and is given by 

M.W = ( f ( (‘> . , ) 6 «« (81) 

\ -™{jk)p(]k) m(j k )I I 


C.l Modal Integrals for the Individual Bodies 

Defined below are a set of modal integrals for the k th body which simplify the computation 
of the modal mass matrix M m (k) and the bias vector b m (k). These modal integrals can 
be computed as a part of the finite-element structural analysis of the individual bodies. 
n *( fc ) 

= E m 0'») 


pS = E + lo (k,j k )) e S 3 

J=lk 

A n <(*) 

P\( r ) = [l/m(i)J J2 m 0'07rW € ft 3 


E\r) ± Y. m Uk)H(k) - p(jk)K(k)} £ ft 3 


Fo k (r) - ^2 J(jk)H(k) 4- Tn(jk){io{k,j k ) + pO*)br(*0 - ^Ukfloi^J^pij^Ki^) £ 

J=u 
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F{(r,s) k 
G k (r,s) k 

Jo k ± 
J?{r) = 

Ji k (r,s) = 

S}(r) = 

S k 2 (r,s) k 
K[{r) k 

Ki(r,s) k 

R k i(r) k 
R k (r,s) k 
R k (q,r,s) = 
W k (r,s) k 
W 2 k (r,s) k 
L k (r,s) k 


n,(k) 

E ”>(j >»;(*)W(*) - pO*)A;(fc)] € st 3 

1 = u 
«»(*) 

E W(fc)]V(ifc)Ai(Jfc) + m(j*)W(*)]%‘fc)72(*) + rn(j k )[\i(k)Yp(j k hl(k) 
j=u 

m(j*)W'Wr7JW € S 1 

«.<*) . . 

- E *70’*) - TO 0*)ft(*»i*)Jo(*,.7*) + p{jk)lo(kJk) + lo(k,jk)p(jk)] € 3f 3x 

>= 1 * 

"•(*) 

~ E m 0*)7r(*)ft>(fc,j*) +P(jk)\ € 3^ x3 

j=U 

n.(fc) 

- e Mbrimm e s 3 * 3 

J=U 

n.(fc) 

E M*Op(iOAj(*)] x «*, a) - J(h)W) e S 3 * 3 

j=l* 

"iW 

E [ m Uk)ptik)K(k)}*il( k ) € ft 3 ’' 3 

J=U 

”<(*) 

E 2/„(*,j»)[>"U)p(i l )A;(*)]'' - JUk)M(t) + MwJUk) e s 3 * 3 

. 7 = 1 * 

n,(/c) 

2[Sf(r)]-- E + J(AW(*)] 

]= u 

n »(fc) 

E 2 7i(*)Mj*)pO*)^(*)] X = 2[^(r,s)]* e ft 35 * 3 

J=l* 

M*) 

E J(h)KW e se 

i=U 

>»»(*) 

E [a;(‘)JU) - m(h)Uk, >*)Ai(t)pO'») Aj(t) 6 S 3 

. 7 = 1 * 

n.(k) 

E -Mhmk)K(k)pu t )KW € S 3 

J=l* 

"*(*) _ 

E W) m O*)pO*)72(fc) 6 3^ 

. 7 = 1 * 

M*) 

E % (k)J(MX’W 6 s 3 

>=u 

n,(k) 

-[1 /m(k)] E ™0*)^(*)p 0*)^(*) 6 ft 3 

J=l* 
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T‘(r, s) ± £ [m(j t mk)P(h) + e s 3 

j= 1* 

"»(*) 

T‘(r, s ) ^ 2 KMflj.Hjf*) - Aj(t) JO'dM*) € »’ 

i=i* 

A n * (fc * r - i 

= EI A j( i )l'h(i»)^(*)#U*) + ^U*)A'(t)]Aj(i)6* 1 


Note that 


Also define, 


G*(r,s) = G fc (s,r) and J k (r,s) = J k (s,r) 


P( k ) = ri+ZMsMs )** 3 


F k (r) £ F 0 fc (r) + Z. F k (r,s)r,(s)e^ 


N k (r) £ [Ji*(r)+ £ J?(r, -),(-)]• 6 »»« 


■?(*) = Jf + E MV) + {^( 0 }*Mr)+ 


Z?=l k) Es=i k) Ji( r , s)v(r)v{s) e X 3 * 3 

S*(r) = S k (r) + Z n s=! k) S k ( r, s)r,(s) G X 3 * 3 


K fc (r) £ K k (r) + Z:=l k) K$(r,s)r](s) G X 3 * 3 
R k (r,s) £ ^(r,s) + E^r^( 9 ,r,s) 7 ? ( 9 )G» 3 ( 83 ) 


C .2 Modal Mass Matrix 


We have from Eq. ( 23 ) that the modal mass matrix of the k th body is given by 

M m (k) = ( U J^ \ Af,(Jfc)[II(*), B m (k)] = ( n ‘( k ) M *( k ) u ( k ) n m {k)M,(k)B*(k) 
V B ( k ) ) \ B(k)M.(k)Tl(k) B(k)M.(k)B-(k) 

_ ( M m( k ) M m( k ) \ vJT(k)x7I(k) (& 

~ \ M T J{k) M”(k) j 6 ^ (* 

Define the matrices: 


Pi = lPi(l), ■■■p k i(n m (k))}e$ 3 * n ”W 
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F 0 k ± [F*(l), ■■•F k (n m (k))]e&* n "'W 

F k = [F fc (l), ■•■F k (n m (k))] € » 3xnm <*> 

E k = [F*(l), • • • E k (n m (k))] € 5ft 3xnm ^ (85) 

5 Also define the matrix G k £ 3? n ”>( fc ) xn ">(*) s0 that its (r, s) tfc element is given by the modal 
integral G k (r,s). 


Using these matrices, and Eq. (84), it is easy to establish that 


10 



Hence, in block partitioned form 


and C(‘) = 


J(k) m(k)p(k) \ 

—m(k)p(k) m(k)I J 





\ 

M m (k) = 

G k 

RT 

[E k ]* 

15 

F k 

J(k) 

m(k)p(k) 


E k 

■ -m(k)p( k) 

m(k)I j 


/ 


\ 

G k 

R]- 

[£*)- 

F k 

r o 

Jt 0 

m(k)p k 

\ E k 

-m(k)p k 

m(k)I j 


A #»(*) 





\ 

+ 

25 

0 

R-ri- 

0 

Ffrt 

ES'WM + W‘(r)]-W) 

m(k)[p k p(k)] x 


{ 0 

-m(fc)[pf7?(fc)] x 

o / 


,(*) 


30 


/ 



\ 


0 

0 

0 


0 


0 

\ 

0 

0 

0 / 


Ml{k) 


( 86 ) 


The superscript i — 0, 1, 2 in M} n (k) denotes the order of dependency of the terms on the 
deformation variables. 
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C.3 Expression for a m (k) 
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In this section we derive explicit expressions for the Coriolis and centrifugal acceleration 
term a m (k). Since 




it follows from Eq. (12) and Eq. (79) that 

o [n‘(* + i )Ykh + uk) + [ri *(* + i)]V(< t+ i, k) 


*(k + l t k) = 


0 


/ 


<f>(k + 1 , k) 

0 [A*(*+l)f [A‘(* + !)]•/(«*+!, ib) + [jr*(ifc + 1)]- + [A*(* + 
0 0 l{k+l,k) 

V o o o 

Recalling that the spatial velocity of frame Tk is 


V(k) = 


u(k) \ 
v{k) ) 


where w(fc) and v(k ) denote the angular and linear velocity respectively of Tk we have 
that 


n>(*) = ( x ‘ {k) ) = ( ) 

\ / \ w(*)7 J (*) / 


And thus 


* m {k + l,k)V m (k + l) = 


u)(k + l)^ w (iit + i) 


~l(tk+ 1» k)u>(k + l)<5 w (tjt+i) + u(k + l)<5t;(<fc+i) 

+L{tk+i)i{t k+u k) + u{k + 1 )l(k + 1, k) ) 

The vector above has been partitioned so that the term on the top corresponds to modal 
accelerations, the term in the middle to the angular acceleration and the term at the 
bottom to the linear acceleration of the body. Also 


/(fc + l,j*+i) — u)(k + l)/(fc + 1, jt+i)+ 


^(j*+i)and 

i(t k+ uk) = w(4 fc+1 )/(<*+i,*) + A v (k)-6 v (d k ) + A w (k)l(O k ,k) 

= [u(k + 1) + M4+i)]/(4+i, *) + A„(*) - 6 v (d k ) + A u (k)l(O k , k) 



48 


where 


Thus 


Av(*) = 


AM 

A v (k) 


= H‘(k)Mk) 


(87) 


l(k + l,k) — l(k + l,f*+i) + l(t k +i, k) 

= u)(k + l)/(& + 1) k) + ^ v (i*+i) + f>w{ik+i)Ktk+h k) + A v (k) 

-S v (d k ) + AMl(O k ,k) 
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Also 


and 


n m (k) = 


H-(k) = 


0 

-n d (*) 


u>(O k ) o 


•° ) 
»m j 

y 


0 A(O k ) 
l(O k ,k ) = u(O k )l(O k ,k)-6 v (d k ) 


Thus we have that 

l 

H-(k)x(k) = 


u(O k )A w (k)-d;(k)6 u (d k ) 


\ A(O k )A v (k) - A(k)6 v (d k ) + A M«O k , k) - l{O k , *)£(C>*)Aw(Jt) ) 
From Eq. (25) and the above expressions it follows that 

/ 0 ( 

( 88 ) 


... dtr(k+ l,Ar)_, „ d W(Jfc) 

a m{k) — — Kn(&+1)H — x(k) — 


dt 


d< 




where 


a mR (k ) = 


u(k + 1)^(<*+,) + u(k)AUk) - u>(O k )6„{dk) 


A(k + l)[u>(* + 1 )l(k + 1 , k) + 2^(4+!)] + [A(t, +1 ) + u(O k )} [u(fc) - v(Ot 

+ [a(& + 1) + ^u»(<t+i)]^u,(tjt+i)/(f*+i, k) 

-[ 2AM-Udk)]s v (d k ) 

u>(k) A UI (A:) 


\ 4- 1)A(& + l)/ 0 (fc + 1, k) + [<!>(£ 1) + <2>(A:)][u(fc) — v(0 *)] 

— ™" 
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+ 

u >(k + 1 ) [uj ( /: + l)[6|(i*+i) — Si(dk)] + 26„((fc+i)] (89) 

+ [«„((*+,) - L(d k )} Mt) - ®(Of)](»°) 

■+■ &{k + 1 )^L(^Jfc+i )^o(^*+i i k) — 2A u ,(A:)^ v (<ifc) a i nR (fc) 

/ 0 


+ 


\ Uj(k + l)^u/(^fc+l )^;(tjH-i) + fiui(tk+i)bu>{tk+i)lo{tk, k — 1) + b w (d k )6 v (d k ) ) 


•LW 
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+ 


(91) 




< R w 

In the above, 0^(1) denotes the deformation independent part of the Coriolis 
acceleration, while a^^k), a^^k) and a^/?(k) denote the parts whose dependency on 
the deformation is up to first, second and third order respectively. 

C.4 Expression for b m (k) 


We have from Eq. (28) that 

d[IP(*), <f> m {k,j k )] 


a (jk) = 


d t 


V m (k) = W(k)f,(k) + <t>‘(k,j k )V(k) 


Since, 


i{k,jk) = U>(k)l(k,j k ) + 6 v (j k ) 


it follows that 


a(j)t) = 


mujk) 


\ u(k)[Q(k)l(k,j k ) + 26 v (j k )} 


Also from Eq. (31) we have that 


/ 


Kjk) = 


u(j k )J{jkP(j k ) 


\ ™(j k )u(j k )u(j k )p{j k ) ) 
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Thus, 


b{jk) + M t {j k )a{jk) 

( u{j k )J(jk)u(jk) + J{jk)u(k)6 w (j k ) + m(jk)pUk)u(k) [u(k)l{k,jk) + 2^0'fc)] \ 


(j*) | - + b u (jk)s w (jk)p(jk) + f>w{jk)u(k)p(j k ) 

+u(k) [u>(fc){/(fc, jk) + p(jk)} + L(jk)p{jk) + 2 S v (j k ) j 


(92 


10 


15 


From Eq. (37) we write 


b m (k) = 




( KM \ 

b k r,(n m {k)) 


b k . 


\ 


bt 


(93) 


) 


We develop expressions for b k (r), 6* and 6* in Eq. (93) below. From Eq. (92) and Eq. (93) 
we have that 


20 





j=u 


25 


30 


-u>-(k)\>(k)J(jk)6 w (jk) 


MjkY'H(k)J(jkMk) 


-6 w (jkrM(k)j(jk)6Ujk) - [H(k)YJ(jk)L(jk)u>(k) 
+ u} m (k)[m(j k )p{jk)\ 3 T {k)]* [ - l(k, jk)u(k) + 2«5„(j A )] 


+ m{ik)[li{k)] m p(jk)L{jkMk) 

- m(j*:)u;*(fc)7^(/:)[ - {l(k, j k ) + p(j k )}u(k) + 26 v (j k )] 

+ m(j t )[7^(A:)3*{^ u ,(;it)^(jjfe )p{jk) + bUjk)u{k)p(j k ) + u>(k)6„{jk)p{jk)} 
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= E"=u -u>-(k)\i(k)j(j k Mk) 

(94) 

- 2u;*(A:)m(jjt)7r(^)^0^)(98) 

+ mO*)[7r(*)]"P(i*)^(i*M*)( 100 ) 

+ Uh) m JUk)H(kMk)( 101) 
-M;0’A J r (*)J(jiO^*)(102) 


+ m(j t )6 w (j' fc ) , 7^(*)p(ifc)^w(ifc)(103) 

+ 6Mkyj{h)K{W» O'fc)(!04) 

+ m (j* ) (7r(^))*^ (j* )w( fc)p(j* ) ( 105) 

+ m (j* ) [7r (^)]*w( Ar)^u,(jfc )p( j* ) ( 106) 

Using the modal integrals defined in Section C.l, the above terms can be expressed 
in the following manner: 

1 n m(^) 

-[97]+102 = -u -'(*) £ Tj(s,r)r](s) 

L 5=1 


2[97]+99+101 = -u.*(*:)^;™ ( * ) [T*( 4 ,r)+W*(r )a )+lV*(*,r)] ^(,) 

94 + 95 = -uj*(k)S k {r)u;(k) 


96 = -u>’(k)N k (r)u}(k)(107) 
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103 + 104 = r,»W( ? W(i) 
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100 + 105 = 106 

98 + 100 + 105 + 106 = -2 w’(Jfe) ££} k) F,*(s,r)j 7 (s)Using these, it foUows that 

ttm(fc) Km(k) 

= -^(*)[5‘(r) + iv*(r)]«(il)+ E E !?h,v)iWiW 

9=1 8=1 

n m (fc) 

-«•(*) E [^,r) + T*(s,r) + ^ 

j=a 

= + -«'(*) E <?*('•,*)«*)+ E E 

J = 5 q= 1 5=1 

(108) 


where 


Q k (r,s) t T k (s, r ) + T k {s,r) + W?(r,s) + W k {s,r) + 2F k (s,r) (109) 

Once again from Eq. (92) and Eq. (93) we have that 
"■(*) 


b k = 


3 = 1* 


E Z(jk)J{jkMjk) + J{jk)u{k)8„{jk) + rn(jk)p(jk)^(k)\u;(k)l(k,j k ) + 2<5 t ,(j*)] 

Ik 

+ m (jk)i(jk){ - PUk)u(k)6 w (j k ) + <^>(^) [j>(Ar){/(Ar, j k ) + p{j k )} + 2<5„(>jt)] 
+L(jk)L(jk)p(jk) + 6Ujk)v{k)p(jk) + w(*)^(ijt)p(jjt)| 


25 


30 


E £S*{k) [j(jk) - m(jk){p(jk)l(kjk) + Hjk)p(kJk) + iUk)l(k,jkj)] u;(*)(110) 
-2 m(U)[i(jk) + p(jk)]UjkMk)(lll) 


-J(j*)<Mj*:Mfc)(ll2) 


+ L(jk)J(kP(k)(llS) 


+ l {jk )m{j k )p(j k )L {jkMk){ 114) 


+ u>(k)J{jk)6u,{jk)( H5) 


+ {>v{jk)J(jk)t>w(jk)(l'LQ) 
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+ Kjk)™(jk)L(jk)L(jk)p{jk)(ii’?) 

+ Mh)Kjk)L(jk)u(k)p(j k )(118) 

Once again, using modal integrals, the above terms can be reexpressed in the 
following manner: 


110 

111 


112+113 + 114 + 118 + 119 

115 
116 + 117 

This results in the following expression 


u m (k)J{k)u(k) 

2 £ N k (r)fj(r) u(k) 

r = 1 
n m{k) 

II K k (r)p{r)u>(k) 

r=l 

£ ^( r )»?(r) 

r=l 

n m (k) n m (fc) 

£ £ /2 fc (r,s)^(r)^(s) 

r=l 3=1 


<£ = E [2AT‘(r) + X*(r)] «r)u,(i) 

r=l 

n m(^) n m(^) 

+*(*) E ^(r)«r)+ £ £ Ji*(r,«)i(r)i( i ) 

r=l r=l a=l 


(120) 


( 121 ) 


Using Eq. (92) and Eq. (93) it also follows that 

n.(fc) 

b t = Y, - m Uk)p(jk)u(k)6 u (j k ) + m(j k )u(k)[u(k){l(k,j k ) + p(j k )} + 26 v (j k )] 

j=l* 

+m(j k )6 u ,(j k )6 u ,(j k )p(j k ) + m(jk)S^(jk)u(k)p{j k ) + m(j k )u(k)6„(j k )p(j k ) 


= Z%S -’ntiMhMWMM 122) 

+ m(j k )u{k)u(k){l(k,j k ) + p(jjt)}(123) 

+ Mh)L(jk)L(jk)p(jk)(i24) 


+ 2m(j fc )<2>(fc)6 v (;*)(125) 



54 


+ m(} k )6„(jk)u}(k)p(j k )(126) 

+ m(j fc )w(fc)6 w (j fc )p(; t )(127)Using the modal integrals we have that 


10 


123 = m(k)u(k)Cj(k)p(k) 

n m (k) n m (k) 

124 = m(k) Y, L{ r ,s)v( r )v(s) 

r=l a=l 
Km(k) 

122 + 125 + 126+ 127 = 2u{k) £ E k (r)fi(r) ( 128 ) 

r=l 

and thus 

riTT^fc) 7i m (/r) n m (k) 

b k v = m(k)u(k)u(k)p(k) + 2u(k) ]T E k (r)fi(r) + m(k) £ H s )*l( r )v( s ) 

r— 1 r = l 3=1 

( 129 ) 


15 


20 


Putting together Eq. (108), Eq. (121) and Eq. (129) we have that 

( -uT(*)[sf(l) + #(!+(*) \ 


M*) = 


-«•(*) [Sf(B.(i)) + ^*(n»(i))] W (*) 


u(k)J*u(k) 


m(k)u(k)Lj(k)po(k) 


*£.(*) 


25 


30 


-«•(*) [o*(i,.)iW + {^(i,.) + J?(i, 4 )}i,(«) 


-«*(*) E”+’ [<?‘(n„(*), <).}(<) + {Sj(n ro (t), 3 ) + J*(n„(lk), ,)},(.)] 

ESi 1 *’ [i>(*){j,‘(r) + M‘Wr}l(rM‘) + {2[J,‘(r)]' + A'*(r)}«(r)u,(*) 

+«(*)*{( r)i(r)’ 

u>( A:) |m(A:)(I)(l:)pjT/ + 2E*pj 
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+ 


E n g =i k) E?=I k) T$(q, l ,*)n(q)v{*) 

EZ=l k) Es=l k) Tf (?, »m(*). s)v(g)q(s) 

Er=i k) Es= l k) [v(k)J 2 k (r, s)u>(k)T}(r)ri(s) + J 2 fc (r, s)u(k)v( r )v(s) 

+R${r,s)fi{r)ri{s)\ 


m(k) E:=i (k) Mr, s)g(rWs) 


^ m (k) 


10 


15 


+ 


Ea=l k) E^l k) Z n sd k) *$(<1, r, sHq)r,(r)f,(s) 


20 




(130) 
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CONTROLLING FLEXIBLE ROBOT ARMS USING A HIGH SPEED 

DYNAMICS PROCESS 

ABSTRACT OF THE INVENTION 
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A robot manipulator controller for a flexible manipulator arm having plural bodies con- 
nected at respective movable hinges and flexible in plural deformation modes correspond- 
ing to respective modal spatial influence vectors relating deformations of plural spaced 
nodes of respective bodies to the plural deformation modes, operates by computing ar- 
ticulated body quantities for each of the bodies from respective modal spatial influence 
vectors, obtaining specified body forces for each of the bodies, and computing modal 
deformation accelerations of the nodes and hinge accelerations of the hinges from the 
specified body forces, from the articulated body quantities and from the modal spatial 
influence vectors. In one embodiment of the invention, the controller further operates by 
comparing the accelerations thus computed to desired manipulator motion to determine 
a motion discrepancy, and correcting the specified body forces so as to reduce the motion 
discrepancy. 

The manipulator bodies and hinges are characterized by respective vectors of 
deformation and hinge configuration variables, and computing modal deformation accel- 
erations and hinge accelerations is carried out for each one of the bodies beginning with 
the outermost body by computing a residual body force from a residual body force of 
a previous body and from the vector of deformation and hinge configuration variables, 
computing a resultant hinge acceleration from the body force, the residual body force 
and the articulated hinge inertia, and then, for each one of the bodies beginning with 
the innermost body, by computing a modal body acceleration from a modal body ac- 
celeration of a previous body, computing a modal deformation acceleration and hinge 
acceleration from the resulting hinge acceleration and from the modal body acceleration 
transformed by the body to hinge force operator. The residual body force is revised 
based upon the resultant hinge force. The modal body accleration is revised based upon 
the modal deformation and hinge acceleration. 
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